
#include "removeNan.h"
namespace perception
{
    namespace lidar_preprocess
    {
        template <typename T1, typename T2, typename T3>
        void RemoveNan<T1, T2, T3>::Process(typename pcl::PointCloud<T1>::Ptr &data_in, typename pcl::PointCloud<T2>::Ptr &data_out, T3 &config)
        {
            if (data_in->points.empty())
            {
                std::cout << "no lidar points input" << std::endl;
                return;
            }
            std::vector<int> mapping;
            typename pcl::PointCloud<T2>::Ptr cloud(new pcl::PointCloud<T2>);
            pcl::copyPointCloud(*data_in, *cloud);
            pcl::removeNaNFromPointCloud(*cloud, *data_out, mapping);
        }
        INSTANTIATE_PRODUCT(INSTANTIATE_RemoveNan, LIDAR_PREPROCESS_TYPES)
    }
}
